package edu.wpi.first.wpilibj.util;

import java.util.Collection;
import java.util.Iterator;
import java.util.Set;

public class ArraySet <T> implements Set<T> {

    private T[] objectArray;
    private ArrayQueue<Integer> availableIndexes;
    private int topIndex = 0;

    public ArraySet() {
        this(8);
    }
    
    public ArraySet(Collection<? extends T> c){
        this (c.size());
        addAll (c);
    }

    @SuppressWarnings("unchecked")
    public ArraySet(int initialCapacity) {
        availableIndexes = new ArrayQueue<Integer>(initialCapacity / 3);
        objectArray = (T[]) new Object[initialCapacity];
    }

    @Override
    @SuppressWarnings("unchecked")
    public boolean add(T e) {
        if (contains(e)) {
            return false;
        }
        if (availableIndexes.isEmpty()) {
            if (topIndex >= objectArray.length) {
                T[] resizeArray = objectArray;
                objectArray = (T[]) new Object[resizeArray.length + 10];
                System.arraycopy(resizeArray, 0, objectArray, 0, resizeArray.length);
            }
            objectArray[topIndex] = e;
            topIndex++;
        } else {
            objectArray[availableIndexes.poll()] = e;
        }
        return true;
    }

    @Override
    public boolean addAll(Collection<? extends T> c) {
        boolean b = true;
        for (Iterator<? extends T> it = c.iterator(); it.hasNext();) {
            T next = it.next();
            b = b && add(next);
        }
        return b;
    }

    @Override
    public void clear() {
        topIndex = 0;
        availableIndexes.clear();
        for (int x = 0; x < objectArray.length; x++) {
            objectArray[x] = null;
        }
    }

    @Override
    public boolean contains(Object o) {
        for (int x = 0; x < objectArray.length; x++) {
            if (objectArray[x] == o) {
                return true;
            }
        }
        return false;
    }

    @Override
    public boolean containsAll(Collection<?> c) {
        boolean b = true;
        for (Iterator<? extends Object> it = c.iterator(); it.hasNext();) {
            b = contains((Object) it.next()) && b;
        }
        return b;
    }

    @Override
    public boolean isEmpty() {
        return topIndex == 0;
    }

    @Override
    public Iterator<T> iterator() {
        return new Iterator<T>() {

            int x;

            @Override
            public boolean hasNext() {
                return x < topIndex;
            }

            @Override
            public T next() {
                for (; objectArray[x] == null && hasNext(); x++);
                return objectArray[x++];
            }

            @Override
            public void remove() {
                ArraySet.this.remove(next());
            }
        };
    }

    @Override
    public boolean remove(Object o) {
        if (o == null) {
            return false;
        }
        for (int x = 0; x < objectArray.length; x++) {
            if (objectArray[x].equals(o)) {
                objectArray[x] = null;
                if (x == topIndex - 1) {
                    topIndex--;
                } else {
                    availableIndexes.add(x);
                    for (int y = topIndex - 1; objectArray[y] == null && y != 0; y--) {
                        topIndex--;
                    }
                }
                return true;
            }
        }
        return false;
    }

    @Override
    public boolean removeAll(Collection<?> c) {
        boolean b = true;
        for (Iterator<?> it = c.iterator(); it.hasNext();) {
            b = b && remove(it.next());
        }
        return b;
    }

    @Override
    public boolean retainAll(Collection<?> c) {
        throw new UnsupportedOperationException("Not supported yet.");
    }

    @Override
    public int size() {
        return topIndex + 1 - availableIndexes.size();
    }

    @Override
    public Object[] toArray() {
        Object[] oa = new Object[size()];
        int x = 0;
        for (T t : objectArray) {
            oa[x] = t;
            x++;
        }
        return oa;
    }

    @Override
    public <T> T[] toArray(T[] a) {
        System.arraycopy(objectArray, 0, a, 0, objectArray.length);
        return a;
    }
}